#include "Interface.h"

namespace seekDot
{
	Interface::Interface(ros::NodeHandle &n)
	:n(n),it(n), img_num(0)
	{
		  //Parameter for marker frame
		  n.param<std::string>("marker_frame_id", marker_frame_id, "/odom");

		  std::cout<<"[seekdot interface][marker_frame_id]: "<<marker_frame_id<<std::endl;


		  //Publisher
		  image_display = it.advertiseCamera("display/image_raw", 1000, 1);
		  image_save =  it.advertiseCamera("save/image_raw", 1000,1);

		  //Subscribe
		  pose_in = n.subscribe<nav_msgs::Odometry>("/pose", 1, &Interface::processPose,this);
		  image_in = it.subscribeCamera<Interface>("/image",1000, &Interface::processImage,this);

		  //Marker Server
		  marker_server.reset(new interactive_markers::InteractiveMarkerServer("seekdot_interface/dot_locations"));

		  //Sound client
		  sndClient.reset(new sound_play::SoundClient(n,"/robotsound"));

		  //Init Menu of Picture Markers
		  initFoundMenu();
	}

	Interface::~Interface()
	{
		// TODO Auto-generated destructor stub
	}

	void Interface::processPose(const nav_msgs::Odometry::ConstPtr& msg)
	{
		last_pose=msg->pose;
	}

	void Interface::processImage(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& camera_info)
	{
		img_num++;
		double z_pos=0;
		//convert number to string
		std::ostringstream numStr;
		numStr<<img_num;

		//Say how much finds we have
		std::string saystr;
		if(img_num == 1)
		{
			saystr=" find";
		}
		else
		{
			saystr=" finds";
		}
		saystr=numStr.str()+saystr;
		sndClient->say(saystr);


		//Check how many markers are already on the current position
		for (std::map<std::string, found>::iterator iter = found_imgs.begin(); iter != found_imgs.end(); iter++)
		{

			if(abs(last_pose.pose.position.x - iter->second.pose.pose.position.x)<0.2)
			{
				if(abs(last_pose.pose.position.y - iter->second.pose.pose.position.y)<0.2)
				{
					visualization_msgs::InteractiveMarker marker;
					marker_server->get(iter->first, marker);
					std::cout<<"zpos:: "<<marker.pose.position.z<<"::"<<z_pos<<std::endl;
					if(marker.pose.position.z >= z_pos)
					{
						z_pos = marker.pose.position.z+0.04;
					}
				}
			}
		}

		//insert picture, info and pose in map
		found_imgs.insert(std::pair<std::string, found> (numStr.str(),found(*msg, *camera_info, last_pose)));

		//Create Marker Position
		geometry_msgs::Point marker_pos;
		marker_pos.x=last_pose.pose.position.x;
		marker_pos.y=last_pose.pose.position.y;
		marker_pos.z=z_pos;

		//Insert Marker
		insertFoundMarker(numStr.str(),marker_pos);
	}

	visualization_msgs::Marker Interface::makeDisc( visualization_msgs::InteractiveMarker &msg )
	{
	  visualization_msgs::Marker marker;

	  //look of the marker
	  marker.type = visualization_msgs::Marker::CYLINDER;
	  marker.scale.x = msg.scale * 0.2;
	  marker.scale.y = msg.scale * 0.2;
	  marker.scale.z = msg.scale * 0.01;
	  marker.color.r = 1;
	  marker.color.g = 0;
	  marker.color.b = 0;
	  marker.color.a = 1.0;

	  return marker;
	}

	visualization_msgs::InteractiveMarkerControl& Interface::makeBoxControl( visualization_msgs::InteractiveMarker &msg )
	{
		visualization_msgs::InteractiveMarkerControl control;
		control.always_visible = true;
		control.markers.push_back( makeDisc(msg) );
		msg.controls.push_back( control );

		return msg.controls.back();
	}

	visualization_msgs::InteractiveMarker Interface::makeEmptyMarker()
	{
		visualization_msgs::InteractiveMarker int_marker;
		int_marker.header.frame_id = marker_frame_id;
		int_marker.scale = 1;
		int_marker.pose.position=last_pose.pose.position;
		return int_marker;
	}

	void Interface::insertFoundMarker(const std::string& name,const geometry_msgs::Point& position)
	{
		std::cout<<name<<std::endl; //TODO REMOVE

		visualization_msgs::InteractiveMarker int_marker = makeEmptyMarker();
		int_marker.name = name;

		visualization_msgs::InteractiveMarkerControl control;

		control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
		control.always_visible = true;

		control.markers.push_back( makeDisc( int_marker ) );
		int_marker.controls.push_back(control);
		int_marker.pose.position=position;

		marker_server->insert(int_marker);

		//Apply the menu
		menu_handler.apply( *marker_server, name );

		//Apply marker changes
		marker_server->applyChanges();
	}

	void Interface::showFoundPic( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
	{
		std::map< std::string, found >::iterator iter=found_imgs.find(feedback->marker_name);
		image_display.publish(iter->second.msg, iter->second.camera_info, ros::Time::now());
	}

	void Interface::deleteFoundPic( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
	{
		std::cout<<"Deleting Picture:"<<feedback->marker_name<<std::endl;

		//erase marker and picture
		found_imgs.erase(feedback->marker_name);
		marker_server->erase(feedback->marker_name);
		marker_server->applyChanges();
	}

	void Interface::saveFoundPic( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
	{
		std::map< std::string, found >::iterator iter=found_imgs.find(feedback->marker_name);
		image_save.publish(iter->second.msg, iter->second.camera_info, ros::Time::now());
	}

	void Interface::initFoundMenu()
	{
	  show_pic_handle = menu_handler.insert( "Display Finding", boost::bind(&Interface::showFoundPic,this,_1) );
	  save_pic_handle = menu_handler.insert( "Save Image of Finding", boost::bind(&Interface::saveFoundPic,this,_1) );
	  delete_pic_handle = menu_handler.insert( "Delete Finding", boost::bind(&Interface::deleteFoundPic,this,_1) );
	}

}
